// 点的发布
#include <memory>

#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/transform_broadcaster.hpp"
#include <rclcpp/rclcpp.hpp>

class PointBroadcaster : public rclcpp::Node {
public:
  // 构造函数
  PointBroadcaster() : Node("point_broadcaster") {

    // 创建发布者
    publisher_ =
        this->create_publisher<geometry_msgs::msg::PointStamped>("point", 10);

    // 每隔500ms发布一个点
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(500),
        std::bind(&PointBroadcaster::timerCallback, this));

    // 创建TF广播器
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
  }

private:
  // 定时器回调函数
  void timerCallback() {
    // 创建并发布点消息
    auto point_msg = std::make_shared<geometry_msgs::msg::PointStamped>();

    // 设置消息头
    point_msg->header.stamp = this->get_clock()->now();
    point_msg->header.frame_id = "base_link";

    // 设置点的坐标
    point_msg->point.x = 1.0;
    point_msg->point.y = 2.0;
    point_msg->point.z = 3.0;

    // 发布点消息
    publisher_->publish(*point_msg);
    RCLCPP_INFO(this->get_logger(), "Published point: [%.2f, %.2f, %.2f]",
                point_msg->point.x, point_msg->point.y, point_msg->point.z);

    // 发布坐标变换
    geometry_msgs::msg::TransformStamped transform_stamped;
    transform_stamped.header.stamp = point_msg->header.stamp;

    // 设置变换的父坐标系和子坐标系
    transform_stamped.header.frame_id = "base_link";
    transform_stamped.child_frame_id = "point_frame";

    // 设置变换的平移和旋转（这里假设没有旋转）
    transform_stamped.transform.translation.x = point_msg->point.x;
    transform_stamped.transform.translation.y = point_msg->point.y;
    transform_stamped.transform.translation.z = point_msg->point.z;

    // 设置为单位四元数，表示没有旋转
    transform_stamped.transform.rotation.x = 0.0;
    transform_stamped.transform.rotation.y = 0.0;
    transform_stamped.transform.rotation.z = 0.0;
    transform_stamped.transform.rotation.w = 1.0;

    // 发布变换
    tf_broadcaster_->sendTransform(transform_stamped);
  }
  // 发布者、
  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr publisher_;
  // 定时器和
  rclcpp::TimerBase::SharedPtr timer_;
  // TF广播器
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<PointBroadcaster>());
  rclcpp::shutdown();
  return 0;
}
